#include "XBEES2Lib.h"

//	XBee Object
XBee	_xbee = XBee();
//	Buffer for last data received from XBee
uint8_t	XBeeS2_lastDataReceived[MAX_PAYLOAD];
//	Lenght of the buffer
uint8_t XBeeS2_lenghtDataReceived;

bool	_xbee_busy = false;


char XBeeS2_Start( ){
	#ifdef DEBUG_XBEE
		Serial.print("\tStarted XBee S2: { ");
	#endif

	_xbee.setSerial(Serial3);

	#ifdef DEBUG_XBEE
		Serial.println(" }");
	#endif

	_xbee_busy = false;

	return 1;
}

bool XBeeS2_SendDataToAddrXBee (uint32_t _msb, uint32_t _lsb, uint8_t* _data, uint8_t _size){
	XBeeAddress64 sendAddr = XBeeAddress64(_msb, _lsb);
	ZBTxRequest zbTx = ZBTxRequest(sendAddr, _data, sizeof(uint8_t)*_size);
	ZBTxStatusResponse txStatus = ZBTxStatusResponse();
	if (!_xbee_busy)
		_xbee.send(zbTx);

	_xbee_busy = true;

	//	After sending a packet, a response should be available within 500 ms
	if (_xbee.readPacket(500)){
		//	Got a response
		//	Should be a znet tx status 
		if (_xbee.getResponse().getApiId() == ZB_TX_STATUS_RESPONSE){
			_xbee.getResponse().getZBTxStatusResponse(txStatus);
			// get the delivery status, the fifth byte
			if (txStatus.getDeliveryStatus() == SUCCESS) {
				#ifdef DEBUG_XBEE
				Serial.println ("\t<<< Data sent OK Xbee S2");
				#endif
				_xbee_busy = false;
				return true;
			} else {
				#ifdef DEBUG_XBEE
					//Serial.println ("\t>>>\tError enviando dato a Xbee S2");
				#endif
			
				return false;
		}
    }
	} else if (_xbee.getResponse().isError()) {
		//nss.print("Error reading packet.  Error code: ");
		#ifdef DEBUG_XBEE
		//Serial.println ("\t>>>\tError enviando dato a Xbee S2");
		#endif
		return false;
	} else {
		#ifdef DEBUG_XBEE
			// local XBee did not provide a timely TX Status Response -- should not happen
			// Serial.println ("\t<<< Module Xbee S2: FAIL");
		#endif
		_xbee_busy = false;
		return false;
	}
	return false;
}

bool XBeeS2_ReceiveDataFromXbee (){
	XBeeResponse response = XBeeResponse();
	ZBRxResponse rx = ZBRxResponse();

	_xbee.readPacket();
    
    if (_xbee.getResponse().isAvailable()) {
		//	Got data
		if (_xbee.getResponse().getApiId() == ZB_RX_RESPONSE) {
			//	Getting data
			_xbee.getResponse().getZBRxResponse(rx);
			memset(XBeeS2_lastDataReceived, 0, MAX_PAYLOAD);
			XBeeS2_lenghtDataReceived = rx.getDataLength();

			for (int i = 0 ; i < rx.getDataLength() ; i ++){
				XBeeS2_lastDataReceived[i] = rx.getData(i);
			}

			#ifdef DEBUG_XBEE
				Serial.println ("\t<<< Data received OK Xbee S2");
			#endif
			if (_xbee.getResponse().getApiId() == ZB_RX_RESPONSE)
				return true;
			else
				return false;
		} else if (_xbee.getResponse().isError()) {
			XBeeS2_lenghtDataReceived = 0;
			#ifdef DEBUG_XBEE
				Serial.println ("\t<<< Error receiving data Xbee S2");
			#endif
			return false;
		}
    }
	return false;
}

bool XBeeS2_SendArrayToAddrXBee (uint32_t _msb, uint32_t _lsb, uint8_t* _data, uint16_t _size, uint8_t _cmd ){
	uint16_t	n_sends	= _size / MAX_PAYLOAD;
	uint16_t n_samples = _size;
	uint16_t cont = 0;

	if (n_samples > MAX_PAYLOAD){
		#ifdef DEBUG_XBEE
			Serial.print ("\t<<< Sending array XBee S2. Sents: ");
			Serial.println (n_sends);
		#endif
		for (int i = 0 ; i < n_sends ; i ++){
			#ifdef DEBUG_XBEE
				Serial.print ("\t<<< Send: ");
				Serial.println (i+1);
			#endif
			if (n_samples > MAX_PAYLOAD){
				_data[cont] = _cmd;
				while(!XBeeS2_SendDataToAddrXBee(_msb, _lsb, &_data[cont], MAX_PAYLOAD)){	
				}
				#ifdef DEBUG_XBEE
					Serial.println ("\t<<< Waiting for continue cmd XBee S2");
				#endif
				delay(900);
				n_samples -= MAX_PAYLOAD;
				cont += MAX_PAYLOAD;
			}
			else{
				while(!XBeeS2_SendDataToAddrXBee(_msb, _lsb, &_data[cont], n_samples)){
				}
				#ifdef DEBUG_XBEE
					Serial.println ("\t<<< Waiting for continue cmd XBee S2");
				#endif
				delay(900);
			}
		}
	}
	else{
		#ifdef DEBUG_XBEE
			Serial.println ("\t<<< Sending array XBee S2. Unique");
		#endif
		_data[cont] = _cmd;
		while(!XBeeS2_SendDataToAddrXBee (_msb, _lsb, _data, _size)){
		}
	}
	return true;
}

void XBeeS2_ResetDataReceived ( void ){
	memset(XBeeS2_lastDataReceived, 0, MAX_PAYLOAD);
}

bool XBeeS2_WaitContinue ( void ){
	while (!XBeeS2_ReceiveDataFromXbee()){
		switch (XBeeS2_lastDataReceived[0]){
			case CONTINUE:
				return true;	
			default:
				return false;
		}
	}
	return false;
}

bool XBeeS2_CheckXBee (){
	uint8_t atsm_cmd[]={'V','R'};
	AtCommandRequest _ATcmd = AtCommandRequest(atsm_cmd);
	AtCommandResponse _ATrsp = AtCommandResponse();

	_xbee.send(_ATcmd);

	//	After sending a packet, a response should be available within 500 ms
	if (_xbee.readPacket(500)){
		//	Got a response
		//	Should be a znet tx status 
		if (_xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE){
			_xbee.getResponse().getAtCommandResponse(_ATrsp);
			// get the delivery status, the fifth byte
			if (_ATrsp.isOk()) {
				#ifdef DEBUG_XBEE
					Serial.println ("\tModule Xbee S2: OK");
				#endif
				return true;
			} else {
				#ifdef DEBUG_XBEE
					Serial.println ("\tModule Xbee S2: FAIL");
				#endif
				return false;
			}
		}
		else{
			#ifdef DEBUG_XBEE
				Serial.println ("\tModule Xbee S2: FAIL");
			#endif
			return false;
		}
    }
	else {
		#ifdef DEBUG_XBEE
			Serial.println ("\tModule Xbee S2: FAIL");
		#endif
		return false;
	}
}

